
#include <stdio.h>
#include "ros/ros.h"
#include <WarbotsMsg/velocityMsg.h>
#include "std_msgs/Float32.h"
#include "common.h"

#if DEBUG_MODE == 0
#include <roboard.h>  // include this to use the RoBoIO library
#define PULSE_IN rcservo_InPin(RCSERVO_PINS5)
#endif


double getRPS(double* rps_hist, int size);

int main(int argc, char **argv) {
#if DEBUG_MODE == 0	
    roboio_SetRBVer(RB_110);
#endif
    
	
    ros::init(argc,argv, "encoder");
    ros::NodeHandle n;
    ros::Publisher rps_pub = n.advertise<WarbotsMsg::velocityMsg>("encoder_rps",2);
    ros::Rate loop_rate(200);    
    ros::Time curr_time, prev_time, prev_pub_time;
    
    const int num_rps  = 10;	 
    double rps[num_rps];    
    int curr_rps = 0;
    double dt = 0;
    WarbotsMsg::velocityMsg rps_msg;
    bool lock = 0;

#if DEBUG_MODE == 0	
    //initialization
    if (rcservo_Init(RCSERVO_USENOPIN) == false)  // set all PWM/GPIO pins as GPIO mode
    {
        ROS_INFO("ERROR: fail to init RC Servo lib (%s)!\n", roboio_GetErrMsg());
        return -1;
    }
#endif 
   
    prev_time = ros::Time::now();
    curr_time = ros::Time::now();
    prev_pub_time = ros::Time::now();
    
    for (int i = 0; i<num_rps; ++i)
    {
	rps[i] = 0;
    }

    //main loop
    while (ros::ok())
    { 
        //update current time
	curr_time = ros::Time::now();
	dt = (curr_time - prev_time).toSec();

#if DEBUG_MODE == 0	
	//time out; cannot detect any pulse
	if (dt > 0.1)
	{
		prev_time = curr_time;
		rps[curr_rps] = 0;   		//motor essentially not moving
		
		//next rps slot
		if (curr_rps < (num_rps-1))
			++curr_rps;
		else
			curr_rps = 0;
		
	}
	//when sees a pulse from the encoder	
	else if (PULSE_IN == 1 && lock == 0 )
	{
		//dt = (curr_time - prev_time).toSec();
		prev_time = curr_time;

		

		if (dt == 0)
		 	rps[curr_rps] = 999999; //error, infinite speed
		else 
			rps[curr_rps] = (1.0/6.0)/dt; //6 pulses per revolution
	
		lock = 1;

		//next rps slot
		if (curr_rps < (num_rps-1))
			++curr_rps;
		else
			curr_rps = 0;
	
	}
    	//wait until the pulse goes low first
	else if (PULSE_IN == 0)
	{
		lock = 0;
	}
#endif
	//publish messages at a constant frequency regardless
	if ((curr_time - prev_pub_time).toSec() >0.02)
	{
		rps_msg.v1 = getRPS(rps, num_rps);
		rps_pub.publish(rps_msg);
		
		//ROS_INFO("Encoder: [%f]", rps_msg.data);
		prev_pub_time = curr_time;
	}
	ros::spinOnce();

	loop_rate.sleep();
	
    }
   
    return 0;
}

double getRPS(double* rps_hist, int size)
{
	double RPS = 0;	
	for (int i = 0; i<size; i++)
	{
		RPS += rps_hist[i];
	}
	return RPS/size;
	
}

